//package com.hitqz.robot.dispatch.netty.util;
//
//import cn.hutool.core.collection.CollUtil;
//import cn.hutool.core.collection.CollectionUtil;
//import cn.hutool.core.util.StrUtil;
//import cn.hutool.extra.spring.SpringUtil;
//import com.baomidou.mybatisplus.core.toolkit.Wrappers;
//import com.google.common.cache.Cache;
//import com.google.common.collect.HashBasedTable;
//import com.google.common.collect.Maps;
//import com.google.common.collect.Table;
//import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
//import com.hitqz.robot.api.business.entity.RobotEntity;
//import com.hitqz.robot.api.business.entity.RobotGroupEntity;
//import com.hitqz.robot.api.business.entity.RobotMapPosEntity;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
//import com.hitqz.robot.biz.service.RobotMapPosService;
//import com.hitqz.robot.dispatch.netty.model.RobotCommonPathDto;
//import com.hitqz.robot.dispatch.netty.model.RobotLockLevelDto;
//import com.hitqz.robot.dispatch.netty.model.RobotLockPos;
//import lombok.extern.slf4j.Slf4j;
//import org.locationtech.jts.geom.Point;
//import org.locationtech.jts.geom.Polygon;
//
//import java.util.*;
//import java.util.concurrent.atomic.AtomicReference;
//
///**
// * 公共路径算法
// * @author xupkun
// * @date 2024/9/5
// */
//@Slf4j
//public class PublicPathStrategyUtil {
//
//    private static String lockA = "path_list_lock";
//
//    public static Map<String, LinkedList> ROBOT_PATH_LIST = Maps.newConcurrentMap();
//
//
//    public static Map<String, String> ROBOT_LAST_POS = Maps.newConcurrentMap();
//
//    public static Map<String, String> ROBOT_NEXT_POS = Maps.newConcurrentMap();
//
//    public static Map<String, String> ROBOT_ARRIVE_POS = Maps.newConcurrentMap();
//
//    public static Map<String, Integer> ROBOT_COMMON_PATH_STATUS = Maps.newConcurrentMap();
//
//    /**
//     * 记录是否有路径
//     * 相同 groupId, pos_code, robotSn
//     */
//    public static Table<Integer, String, String> ROBOT_LOCK_TARGET_POS = HashBasedTable.create();
//
//    /**
//     * 记录前往-的记录
//     */
//    private static Map<String, RobotApplyPathDto> ROBOT_TARGET_POS_MAP = Maps.newHashMap();
//
//    public static Table<Integer, String, RobotCommonPathDto> COMMON_PATH_MAP = HashBasedTable.create();
//
//    public static Map<String, Boolean> ROBOT_STOP_FLAG = Maps.newConcurrentMap();
//
//    public static Map<String, Map<String, Boolean>> ROBOT_COMMON_PATH_STOP_FLAG = Maps.newConcurrentMap();
//
//
//    /**
//     * 机器人进入公共路径状态  未进入
//     */
//    public static Integer ROBOT_COMMON_PATH_NAVER_IN=0;
//    /**
//     * 机器人进入公共路径状态  已经进入
//     */
//    public static Integer ROBOT_COMMON_PATH_IN=1;
//    /**
//     * 机器人进入公共路径状态  离开
//     */
//    public static Integer ROBOT_COMMON_PATH_LEAVE=2;
//
//    /**
//     * 区域类型：1-公共区域锁控制
//     */
//    public static String AREA_TYPE_LOCK="1";
//    /**
//     * 区域类型：2-开关避障区域
//     */
//    public static String AREA_TYPE_SENSOR="2";
//
//    /**
//     * 计算
//     * @param
//     */
//
//    public static void calculateDispatch(Integer groupId) {
//        List<RobotLockLevelDto> lockLevelDtos = PublicPathStrategyUtil.calEnteringPublicDistance(COMMON_PATH_MAP.row(groupId));
//        // 排序
//        PublicPathStrategyUtil.orderSamePath(groupId, lockLevelDtos);
//        // 暂停or回复
//        for (String robotSn : ROBOT_COMMON_PATH_STOP_FLAG.keySet()) {
//            try {
//                //AGV暂停/继续判断逻辑及控制
//                Map<String, Boolean> commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//                if (commonPthStop == null || commonPthStop.isEmpty()) {
//                    continue;
//                }
//                Boolean stopFlag = false;
//                Iterator iterator = commonPthStop.entrySet().iterator();
//                while (iterator.hasNext()) {
//                    Map.Entry entry = (Map.Entry) iterator.next();
//                    Boolean value = (Boolean) entry.getValue();
//                    String keyName = (String) entry.getKey();
//                    if (COMMON_PATH_MAP.get(groupId, keyName) != null || "WAIT_CLOSE_RASTER".equalsIgnoreCase(keyName)) {
//                        stopFlag = (stopFlag || value) ? true : false;
//                    } else {
//                        iterator.remove();
//                    }
//                }
//                if (stopFlag) {
//                    if (ROBOT_STOP_FLAG.get(robotSn) != null && !ROBOT_STOP_FLAG.get(robotSn)) {
//                        log.info("********{}-暂停行驶，stopFlag:{},ROBOT_STOP_FLAG:{}********", robotSn, stopFlag, ROBOT_STOP_FLAG.get(robotSn));
//                        PublicPathStrategyUtil.changeNavStatus(robotSn, 1);
//                    }
//                } else {
//                    //继续
//                    if (ROBOT_STOP_FLAG.get(robotSn) != null && ROBOT_STOP_FLAG.get(robotSn)) {
//                        log.info("********{}-继续行驶，stopFlag:{},ROBOT_STOP_FLAG:{}********", robotSn, stopFlag, ROBOT_STOP_FLAG.get(robotSn));
//                        PublicPathStrategyUtil.changeNavStatus(robotSn, 2);
//                    }
//                }
//                ROBOT_STOP_FLAG.put(robotSn, stopFlag);
//            } catch (Exception e) {
//                log.error("**********控制机器人暂停/继续逻辑异常:{}**********", e);
//            }
//        }
//    }
//
//
//    public static void orderSamePath(Integer groupId, List<RobotLockLevelDto> commonPathLockLevelList) {
//        String firstRobotSn = "", samePathOtherRobot = "";
//        if (!commonPathLockLevelList.isEmpty() && commonPathLockLevelList.size() > 0) {
//            //申请路径锁，通过公共路径排序集，按lockLevelMin、lockLevelMax 从小到大排序，排在最前的公共路径，按lockLevelMin对应的AGV拥有最先行驶权
//            firstRobotSn = commonPathLockLevelList.get(0).getRobotSnMin();//排在第一的公共路径，权限最高的AGV，可优先行驶
//            samePathOtherRobot = commonPathLockLevelList.get(0).getRobotSnMax();//排在第一的公共路径，第二台AGV
//            for (RobotLockLevelDto dto : CollUtil.emptyIfNull(commonPathLockLevelList)) {
//                if (dto.getCommonPathDto().getSameDirection()) {
//                    //同向公共路径，按AGV先后申请权限
//                    if (dto.getRobotSnMin().equalsIgnoreCase(firstRobotSn)) {
//                        //权限最高的AGV，优先申请，可以删除公共点位锁的数据，
//                        applySameDirectionLock(groupId, ROBOT_PATH_LIST.get(dto.getRobotSnMin()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin(),
//                                true,
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin());
//                    } else {
//                        applySameDirectionLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMin()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin(),
//                                false,
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin());
//                    }
//                    if (dto.getRobotSnMax().equalsIgnoreCase(firstRobotSn)) {
//                        applySameDirectionLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMax()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMax(),
//                                dto.getLockLevelMax(),
//                                true,
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin());
//                    } else {
//                        applySameDirectionLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMax()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMax(),
//                                dto.getLockLevelMax(),
//                                false,
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin());
//                    }
//                } else {
//                    //反向公共路径
//                    if (dto.getRobotSnMin().equalsIgnoreCase(firstRobotSn)) {
//                        applyReverseLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMin()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin(),
//                                true, false);
//                    } else if (dto.getRobotSnMin().equalsIgnoreCase(samePathOtherRobot)) {
//                        applyReverseLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMin()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin(),
//                                false, true);
//                    } else {
//                        applyReverseLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMin()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMin(),
//                                dto.getLockLevelMin(),
//                                false, false);
//                    }
//                    if (dto.getRobotSnMax().equalsIgnoreCase(firstRobotSn)) {
//                        applyReverseLock(groupId, ROBOT_PATH_LIST.get(dto.getRobotSnMax()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMax(),
//                                dto.getLockLevelMax(),
//                                true, false);
//                    } else if (dto.getRobotSnMax().equalsIgnoreCase(samePathOtherRobot)) {
//                        applyReverseLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMax()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMax(),
//                                dto.getLockLevelMax(),
//                                false, true);
//                    } else {
//                        applyReverseLock(groupId,ROBOT_PATH_LIST.get(dto.getRobotSnMax()),
//                                dto.getCommonPathDto(),
//                                dto.getRobotSnMax(),
//                                dto.getLockLevelMax(),
//                                false, false);
//                    }
//                }
//            }
//        }
//    }
//
//
//
//    /**
//     * 申请公共路径锁资源 —— 同向
//     *
//     * @param robotPathList  路径点集合
//     * @param commonPathDto  公共路径对象
//     * @param robotSn        AGV编码
//     * @param lockLevel      路权（距离公共路径起点的距离）
//     * @param deleteFlag     是否可以删除点位锁表的数据
//     * @param frontRobotSn   前一台AGV
//     * @param frontRobotLock 前一台AGV的路权
//     */
//    private static void applySameDirectionLock(Integer groupId, List<String> robotPathList, RobotCommonPathDto commonPathDto, String robotSn, Double lockLevel, Boolean deleteFlag, String frontRobotSn, Double frontRobotLock) {
//        try {
//            if (robotPathList == null || robotPathList.size() <= 1) {
//                return;
//            }
//            Map<String, Boolean> commonPthStop = new HashMap<>();
//            if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//                commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//            }
//            if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) {
//                commonPthStop.put(commonPathDto.getCommonPathName(), false);
//            }
//            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//            //AGV离开公共路径，则删除相关点位锁，返回
//            if (ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()) != null
//                    && Objects.equals(ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()), ROBOT_COMMON_PATH_LEAVE)) {
//                return;
//            }
//            if (Double.compare(lockLevel, Double.valueOf(7)) > 0) {
//                return;
//            }
//            Boolean frontRobotStop = false;
//            //如果同向前一台机器人已经进入公共路径，且停止前进，则当前机器人也停止；
//            if (!robotSn.equalsIgnoreCase(frontRobotSn) && Double.compare(frontRobotLock, 0.0) == 0 && ROBOT_COMMON_PATH_STOP_FLAG.containsKey(frontRobotSn)) {
//                frontRobotStop = ROBOT_COMMON_PATH_STOP_FLAG.get(frontRobotSn).get(commonPathDto.getCommonPathName());
//            }
//            if (frontRobotStop != null && frontRobotStop) {
//                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//                }
//                if ((COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) && (!commonPthStop.containsKey(commonPathDto.getCommonPathName()) || !commonPthStop.get(commonPathDto.getCommonPathName()))) {
//                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
//                    log.info("#########前一台机器人{}已经进入公共路径，且停止前进，则当前机器人{}也暂停。#########", frontRobotSn, robotSn);
//                }
//                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//                return;
//            }
//            Double distLock = 0.0;
//            //同向行驶，锁前进点位，尝试插入，成功，则可前进，失败，则停止前进
//            List<RobotLockPos> lockPosList = new ArrayList<>();
//            RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
//            for (int i = 0; i < robotPathList.size(); i++) {
//                String pos = robotPathList.get(i);
//                RobotLockPos lockPos = new RobotLockPos();
//                lockPos.setRobotSn(robotSn);
//                lockPos.setCommonPath(commonPathDto.getCommonPathName());
//                lockPos.setLockPos(pos);
//                lockPosList.add(lockPos);
//                if (i < robotPathList.size() - 1) {
//                    RobotMapPosEntity current = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                            .eq(RobotMapPosEntity::getCode, pos).last(" limit 1"));
//                    RobotMapPosEntity next = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                            .eq(RobotMapPosEntity::getCode, robotPathList.get(i + 1)).last(" limit 1"));
//                    distLock += Math.abs(Math.sqrt((current.getPosY() - next.getPosX()) *
//                            (current.getPosX() - next.getPosX()) + (current.getPosY() - next.getPosY()) * (current.getPosY() - next.getPosY())));
//                    if (Double.compare(distLock, Double.valueOf(7.0)) >= 0) {
//                        break;
//                    }
//                }
//            }
//            //申请路径点位锁失败，则暂停标记为TRUE,并返回
//            //if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//            //    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//            //}
//            //if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName()) != null) {
//            //    commonPthStop.put(commonPathDto.getCommonPathName(), true);
//            //}
//            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//        } catch (Exception e) {
//            log.error("申请锁资源程序异常：{}", e);
//        }
//    }
//
//    /**
//     * 申请公共路径锁资源 —— 反向
//     *
//     * @param robotPathList 路径点集合
//     * @param commonPathDto 公共路径对象
//     * @param robotSn       AGV编码
//     * @param lockLevel     路权（距离公共路径起点的距离）
//     * @param deleteFlag    是否可以删除点位锁表的数据
//     * @param stopFlag      暂停标记（如果公共路径的另一台AGV已经优先行驶，则当前AGV直接暂停）
//     */
//    private static void applyReverseLock(Integer groupId, List<String> robotPathList, RobotCommonPathDto commonPathDto, String robotSn, Double lockLevel, Boolean deleteFlag, Boolean stopFlag) {
//        try {
//            if (robotPathList == null || robotPathList.size() <= 1) {
//                return;
//            }
//            Map<String, Boolean> commonPthStop = new HashMap<>();
//            if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//                commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//            }
//            if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null) {
//                commonPthStop.put(commonPathDto.getCommonPathName(), false);
//            }
//            //AGV离开公共路径，直接返回
//            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//            if (ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()) != null
//                    && ROBOT_COMMON_PATH_STATUS.get(robotSn + "_" + commonPathDto.getCommonPathName()) == ROBOT_COMMON_PATH_LEAVE) {
//                //robotLockPathService.deleteByRobotSnAndCommpathName(robotSn, commonPathDto.getCommonPathName());
//                return;
//            }
//            if (Double.compare(lockLevel, Double.valueOf(7.0)) > 0) {
//                return;
//            }
//            //如果公共路径另一台AGV优先行驶，则当前AGV暂停标记stopFlag为true，并且未进入公共路径，则直接暂停标记，并返回
//            if (stopFlag && Double.compare(lockLevel, 0) > 0) {
//                if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//                    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//                }
//                if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null) {
//                    commonPthStop.put(commonPathDto.getCommonPathName(), true);
//                }
//                ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//                return;
//            }
//            //未进入公共路径，申请加锁，如果插入失败，则标记暂停
//            //if (ROBOT_COMMON_PATH_STOP_FLAG.containsKey(robotSn)) {
//            //    commonPthStop = ROBOT_COMMON_PATH_STOP_FLAG.get(robotSn);
//            //}
//            //if (COMMON_PATH_MAP.get(groupId, commonPathDto.getCommonPathName())!= null) {
//            //    commonPthStop.put(commonPathDto.getCommonPathName(), true);
//            //}
//            ROBOT_COMMON_PATH_STOP_FLAG.put(robotSn, commonPthStop);
//        } catch (Exception e) {
//            log.error("申请锁资源程序异常：{}", e);
//        }
//    }
//
//    /**
//     * 计算进入公共路径的距离
//     */
//    public static List<RobotLockLevelDto> calEnteringPublicDistance(Map<String, RobotCommonPathDto> robotCommonPathDtoList)  {
//        List<RobotLockLevelDto> commonPathLockLevelList = new ArrayList<>();
//        List<String> commonPathNameArr = new ArrayList<>();
//        for (String commonPathName :  robotCommonPathDtoList.keySet()) {
//            RobotCommonPathDto commonPathDto = robotCommonPathDtoList.get(commonPathName);
//            if (commonPathDto == null) {
//                continue;
//            }
//            commonPathNameArr.add(commonPathDto.getCommonPathName());
//            String robotA = commonPathDto.getRobotSnA();
//            String robotB = commonPathDto.getRobotSnB();
//            List<String> robotA_pathList = ROBOT_PATH_LIST.get(robotA);
//            List<String> robotB_pathList = ROBOT_PATH_LIST.get(robotB);
//            String robotA_arrive_pos = ROBOT_ARRIVE_POS.get(robotA);
//            String robotA_next_pos = ROBOT_NEXT_POS.get(robotA);
//            String robotA_last_pos = ROBOT_LAST_POS.get(robotA);
//            String robotB_arrive_pos = ROBOT_ARRIVE_POS.get(robotB);
//            String robotB_next_pos = ROBOT_NEXT_POS.get(robotB);
//            String robotB_last_pos = ROBOT_LAST_POS.get(robotB);
//            if (commonPathDto.getAvoidNum() <= 0) {
//                //无避让区， 判断公共路径优先级
//                Double robotA_lockLevel = PublicPathStrategyUtil.getCommonLockLevel(commonPathDto, robotA, robotA_pathList, robotA_arrive_pos, robotA_next_pos);
//                Double robotB_lockLevel = PublicPathStrategyUtil.getCommonLockLevel(commonPathDto, robotB, robotB_pathList, robotB_arrive_pos, robotB_next_pos);
//                RobotLockLevelDto dto = new RobotLockLevelDto();
//                dto.setCommonPathDto(commonPathDto);
//                if (Double.compare(robotA_lockLevel, robotB_lockLevel) < 0) {
//                    dto.setLockLevelMin(robotA_lockLevel);
//                    dto.setRobotSnMin(robotA);
//                    dto.setLockLevelMax(robotB_lockLevel);
//                    dto.setRobotSnMax(robotB);
//                } else if (Double.compare(robotA_lockLevel, 0.0) == 0 && Double.compare(robotB_lockLevel, 0.0) == 0) {
//                    if (commonPathDto.getSameDirection()) {
//                        //同向行驶，如果AGV都进入公共路径，则判断AGV的前后（AGV离公共路径终点近的在前）
//                        Double robotA_dist = getToEndCodeDist(commonPathDto, robotA, robotA_pathList);
//                        Double robotB_dist = getToEndCodeDist(commonPathDto, robotB, robotB_pathList);
//                        if (Double.compare(robotA_dist, robotB_dist) < 0) {
//                            dto.setLockLevelMin(robotA_lockLevel);
//                            dto.setRobotSnMin(robotA);
//                            dto.setLockLevelMax(0.1);
//                            dto.setRobotSnMax(robotB);
//                        } else {
//                            dto.setLockLevelMin(robotB_lockLevel);
//                            dto.setRobotSnMin(robotB);
//                            dto.setLockLevelMax(0.1);
//                            dto.setRobotSnMax(robotA);
//                        }
//                    } else {
//                        //反向行驶，AGV都即将进入公共路径，判断AGV离公共路径起点近的优先行驶
//                        Double robotA_dist = getToStartCodeDist(commonPathDto, robotA, robotA_pathList);
//                        Double robotB_dist = getToStartCodeDist(commonPathDto, robotB, robotB_pathList);
//                        if (Double.compare(robotA_dist, robotB_dist) < 0) {
//                            dto.setLockLevelMin(robotA_lockLevel);
//                            dto.setRobotSnMin(robotA);
//                            dto.setLockLevelMax(0.1);
//                            dto.setRobotSnMax(robotB);
//                        } else if (Double.compare(robotA_dist, robotB_dist) > 0) {
//                            dto.setLockLevelMin(robotB_lockLevel);
//                            dto.setRobotSnMin(robotB);
//                            dto.setLockLevelMax(0.1);
//                            dto.setRobotSnMax(robotA);
//                        } else {
//                            //反向行驶，两台AGV同时进入公共路径，则异常
//                            dto.setLockLevelMin(robotB_lockLevel);
//                            dto.setRobotSnMin(robotB);
//                            dto.setLockLevelMax(robotA_lockLevel);
//                            dto.setRobotSnMax(robotA);
//                            if (ROBOT_STOP_FLAG.get(robotA) != null && !ROBOT_STOP_FLAG.get(robotA)) {
//                                log.error("************AGV控制异常：{}-{}同时进入公共路径-{},{}暂停等待*************", robotA, robotB, commonPathDto.getCommonPathPos(), robotA);
//                                PublicPathStrategyUtil.changeNavStatus(robotA, 1);
//                                ROBOT_STOP_FLAG.put(robotA, true);
//                            }
//                            if (ROBOT_STOP_FLAG.get(robotB) != null && !ROBOT_STOP_FLAG.get(robotB)) {
//                                log.error("************AGV控制异常：{}-{}同时进入公共路径-{},{}暂停等待*************", robotA, robotB, commonPathDto.getCommonPathPos(), robotB);
//                                PublicPathStrategyUtil.changeNavStatus(robotB, 1);
//                                ROBOT_STOP_FLAG.put(robotB, true);
//                            }
//                            continue;
//                        }
//                    }
//                } else {
//                    dto.setLockLevelMin(robotB_lockLevel);
//                    dto.setRobotSnMin(robotB);
//                    dto.setLockLevelMax(robotA_lockLevel);
//                    dto.setRobotSnMax(robotA);
//                }
//                dto.setSamePathLevel(2);
//                if (commonPathDto.getSameDirection()) {
//                    dto.setSamePathLevel(3);
//                }
//                commonPathLockLevelList.add(dto);
//            }
//        }
//        return commonPathLockLevelList;
//    }
//
//
//
//
//    public static void getOrSetPathList(String robotSn, LinkedList<String> robotPathList, Double posx, Double posy, Boolean setFlag) {
//        synchronized (lockA) {
//            if (setFlag) {
//                ROBOT_PATH_LIST.put(robotSn, robotPathList);
//            } else {
//                List<String> currentPathList = ROBOT_PATH_LIST.get(robotSn);
//                if (currentPathList == null || currentPathList.isEmpty()) {
//                    return;
//                }
//
//                // 到达点位
//                Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
//                if (posInfoDtoCache == null) {
//                    return;
//                }
//                RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
//                if (robotClientSummaryDto == null) {
//                    return;
//                }
//                String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
//                if (StrUtil.isEmpty(lastArrivePos)) {
//                    return;
//                }
//
//                int pathListSize = currentPathList.size();
//                int arrivePosIndex = -1;
//                for (int i = 0; i < pathListSize; i++) {
//                    if (lastArrivePos.equalsIgnoreCase(currentPathList.get(i))) {
//                        arrivePosIndex = i;
//                        break;
//                    }
//                }
//
//                if (arrivePosIndex == -1) {
//                    return;
//                }
//
//                String arrivePos = currentPathList.get(arrivePosIndex);
//                ROBOT_ARRIVE_POS.put(robotSn, arrivePos);
//                if (pathListSize > (arrivePosIndex + 1)) {
//                    ROBOT_NEXT_POS.put(robotSn, currentPathList.get(arrivePosIndex + 1));
//                }
//                String lastPos = (arrivePosIndex > 0) ? currentPathList.get(arrivePosIndex - 1) : "";
//                ROBOT_LAST_POS.put(robotSn, lastPos);
//                currentPathList.remove(arrivePosIndex);
//                ROBOT_PATH_LIST.put(robotSn, new LinkedList<>(currentPathList));
//                log.info("****{}-机器人经过点位-{}", robotSn, arrivePos);
//                //log.info("剩下的位置为: {}", currentPathList.toString());
//            }
//        }
//    }
//
//
//
//    private static Double getCommonLockLevel(RobotCommonPathDto commonPathDto, String robotSn, List<String> robotPathList, String robotA_arrivePos, String robotA_nextPos) {
//        String key = robotSn + "_" + commonPathDto.getCommonPathName();
//        //Map<String, Double> robotA_currPos = MAP_CURR_POSE.get(robotSn);
//        Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
//        if (posInfoDtoCache == null) {
//            return null;
//        }
//        if (ROBOT_COMMON_PATH_STATUS.get(key) == 0) {
//            if (commonPathDto.getCommonPathPos().contains(robotA_arrivePos + "-")) {
//                //已进入，直接返回 0.0D
//                ROBOT_COMMON_PATH_STATUS.put(key, 1);
//                return 0.0D;
//            }
//            RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
//            if (robotClientSummaryDto == null) {
//                return null;
//            }
//            String startPos = commonPathDto.getStartPos();
//            String endPos = commonPathDto.getEndPos();
//            Double dist = 0.0;
//            //是否离开公共路径
//            Boolean leave = true;
//            if (robotPathList != null && !robotPathList.isEmpty() && robotPathList.size() > 1) {
//                //计算AGV当前位置到公共路径起点的距离
//                Point currentPoint = JtsUtil.getPoint(robotClientSummaryDto.getPosition().getLaserPos().getX(),
//                        robotClientSummaryDto.getPosition().getLaserPos().getY());
//                RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
//                RobotMapPosEntity robotMapPosEntity = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                        .eq(RobotMapPosEntity::getCode, robotPathList.get(1)).last(" limit 1"));
//                Point publicDistance = JtsUtil.getPoint(robotMapPosEntity.getPosX(), robotMapPosEntity.getPosY());
//                dist = currentPoint.distance(publicDistance);
//                for (int i = 1; i < robotPathList.size() - 1; i++) {
//                    String pos = robotPathList.get(i);
//                    if (pos.equalsIgnoreCase(startPos)
//                            || pos.equalsIgnoreCase(endPos)) {
//                        leave = false;
//                        break;
//                    }
//                    robotMapPosEntity = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                            .eq(RobotMapPosEntity::getCode, pos).last(" limit 1"));
//                    publicDistance = JtsUtil.getPoint(robotMapPosEntity.getPosX(), robotMapPosEntity.getPosY());
//                    dist += currentPoint.distance(publicDistance);
//                }
//            }
//            if (leave) {
//                //AGV已离开公共路径，直接返回 10.0D
//                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_LEAVE);
//                return 10.0D;
//            }
//            if (Double.compare(dist, 1) <= 0) {
//                //如果当前AGV离公共路径起点的距离小于避障距离，则认为已进入，直接返回 0.0D
//                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_IN);
//                dist = 0.0D;
//            }
//            return dist;
//        } else if (Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_IN)) {
//            Boolean startPos = false, endPos = false, leaveFlag = false;
//            //判断AGV是否离开公共路径
//            for (String pathPos : CollUtil.emptyIfNull(robotPathList)) {
//                if (pathPos.equalsIgnoreCase(commonPathDto.getStartPos())) {
//                    startPos = true;
//                }
//                if (pathPos.equalsIgnoreCase(commonPathDto.getEndPos())) {
//                    endPos = true;
//                }
//            }
//            if (startPos && !endPos) {
//
//            } else if (!startPos && endPos) {
//
//            } else if (!startPos && !endPos) {
//                //不包含公共路径点，则为离开公共路径
//                leaveFlag = true;
//            }
//            if (leaveFlag) {
//                //AGV已离开公共路径，直接返回 10.0D
//                ROBOT_COMMON_PATH_STATUS.put(key, ROBOT_COMMON_PATH_LEAVE);
//                return 10.0D;
//            }
//            return 0.0D;
//        } else if (Objects.equals(ROBOT_COMMON_PATH_STATUS.get(key), ROBOT_COMMON_PATH_LEAVE)) {
//            return 10.0D;
//        }
//        return 99D;
//    }
//
//
//    private static Double getToEndCodeDist(RobotCommonPathDto commonPathDto, String robotSn, List<String> robotPathList) {
//        Double distEnd = 0.0;
//        String key = robotSn + "_" + commonPathDto.getCommonPathName();
//        if (ROBOT_COMMON_PATH_STATUS.get(key) != null
//                && ROBOT_COMMON_PATH_STATUS.get(key) == 1) {
//            Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
//            RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
//            if (robotClientSummaryDto == null) {
//                return null;
//            }
//            String endPos = commonPathDto.getEndPos();
//            if (!robotPathList.isEmpty() && robotPathList.size() > 0) {
//                if (robotPathList.get(0).equalsIgnoreCase(endPos)) {
//                    return distEnd;
//                }
//                Point currentPoint = JtsUtil.getPoint(robotClientSummaryDto.getPosition().getLaserPos().getX(),
//                        robotClientSummaryDto.getPosition().getLaserPos().getY());
//                RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
//                RobotMapPosEntity robotMapPosEntity = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                        .eq(RobotMapPosEntity::getCode, robotPathList.get(1)).last(" limit 1"));
//                Point publicDistance = JtsUtil.getPoint(robotMapPosEntity.getPosX(), robotMapPosEntity.getPosY());
//                RobotApplyPathDto robotApplyPathDto = ROBOT_TARGET_POS_MAP.get(robotSn);
//                distEnd = currentPoint.distance(publicDistance);
//                for (int i = 1; i < robotPathList.size() - 1; i++) {
//                    String pos = robotPathList.get(i);
//                    if (pos.equalsIgnoreCase(endPos)) {
//                        break;
//                    } else {
//                        distEnd += robotApplyPathDto.getPathDistances().get(i);
//                    }
//                }
//            }
//        }
//        return distEnd;
//    }
//
//
//    /**
//     * 同向行驶，AGV优先级相同，判断哪台AGV更靠近公共路径终点（AGV在前，先申请）
//     *
//     * @param commonPathDto
//     * @param robotSn
//     * @return
//     */
//    private static Double getToStartCodeDist(RobotCommonPathDto commonPathDto, String robotSn, List<String> robotPathList) {
//        Double distEnd = 0.0;
//        if (!robotPathList.isEmpty() && robotPathList.size() > 0) {
//            if (robotPathList.get(0).equalsIgnoreCase(commonPathDto.getStartPos())
//                    || robotPathList.get(0).equalsIgnoreCase(commonPathDto.getEndPos())) {
//                return distEnd;
//            }
//            Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
//            RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
//            if (robotClientSummaryDto == null) {
//                return null;
//            }
//            Point currentPoint = JtsUtil.getPoint(robotClientSummaryDto.getPosition().getLaserPos().getX(),
//                    robotClientSummaryDto.getPosition().getLaserPos().getY());
//            RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
//            RobotMapPosEntity robotMapPosEntity = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                    .eq(RobotMapPosEntity::getCode, robotPathList.get(1)).last(" limit 1"));
//            Point publicDistance = JtsUtil.getPoint(robotMapPosEntity.getPosX(), robotMapPosEntity.getPosY());
//            RobotApplyPathDto robotApplyPathDto = ROBOT_TARGET_POS_MAP.get(robotSn);
//            distEnd = currentPoint.distance(publicDistance);
//            for (int i = 1; i < robotPathList.size() - 1; i++) {
//                String pos = robotPathList.get(i);
//                if (pos.equalsIgnoreCase(commonPathDto.getStartPos()) || pos.equalsIgnoreCase(commonPathDto.getEndPos())) {
//                    break;
//                }
//                distEnd += robotApplyPathDto.getPathDistances().get(i);
//            }
//        }
//        return distEnd;
//    }
//
//    /**
//     * 一开始进行路径申请
//     * @param robotGroup
//     * @param robotApplyPathDto
//     */
//    public static void applyPath(RobotGroupEntity robotGroup, RobotApplyPathDto robotApplyPathDto) {
//        // 检查目标点是否被占用
//        String targetLockRobot = ROBOT_LOCK_TARGET_POS.get(robotGroup.getId(), robotApplyPathDto.getCode());
//        if (StrUtil.isEmpty(targetLockRobot)) {
//            // 目标点未被锁定 -> 变更为锁定
//            ROBOT_LOCK_TARGET_POS.put(robotGroup.getId(),robotApplyPathDto.getCode(), robotApplyPathDto.getClientId());
//            robotApplyPathDto.setStopFlag(false);
//            ROBOT_TARGET_POS_MAP.put(robotApplyPathDto.getClientId(), robotApplyPathDto);
//        } else {
//            // 判断是否和静止的机器人点位重复，重复的话也算是占用
//            Cache<Integer, List<RobotEntity>> posInfoDtoCache = SpringUtil.getBean("robotDispatch");
//            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//            List<RobotEntity> robotEntityList = posInfoDtoCache.getIfPresent(robotGroup.getId());
//            if (robotEntityList == null) {
//                return;
//            }
//            if (robotEntityList.size() > 1) {
//                robotEntityList = robotEntityList.stream().filter(t -> !(t.getRobotSn().equals(robotApplyPathDto.getClientId()))).toList();
//                AtomicReference<String> lockRobot = new AtomicReference<>("");
//                Point point = JtsUtil.getPoint(robotApplyPathDto.getCodeX(), robotApplyPathDto.getCodeY());
//                robotEntityList.stream().forEach(t -> {
//                    Polygon polygon = robotPosPolygon.getIfPresent(t.getRobotSn());
//                    assert polygon != null;
//                    if (polygon.contains(point)) {
//                        lockRobot.set(t.getRobotSn());
//                    }
//                });
//                // 被占用
//                if (StrUtil.isNotBlank(lockRobot.get())) {
//                    // 下发导航暂停
//                    log.info("下发导航暂停");
//                    PublicPathStrategyUtil.changeNavStatus(robotApplyPathDto.getClientId(), 1);
//                    ROBOT_LOCK_TARGET_POS.put(robotGroup.getId(), robotApplyPathDto.getCode(), robotApplyPathDto.getClientId());
//                    robotApplyPathDto.setStopFlag(true);
//                } else {
//                    ROBOT_LOCK_TARGET_POS.put(robotGroup.getId(), robotApplyPathDto.getCode(), robotApplyPathDto.getClientId());
//                    robotApplyPathDto.setStopFlag(false);
//                }
//            }
//            ROBOT_TARGET_POS_MAP.put(robotApplyPathDto.getClientId(), robotApplyPathDto);
//        }
//    }
//
//    public static void changeNavStatus(String clientId, int status) {
//        SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(clientId, status));
//    }
//
//    /**
//     * 查询公共路径
//     * @param robotSn
//     */
//    public static void findCommonPathThread(Integer groupId,String robotSn) {
//        RobotApplyPathDto firstRobot = ROBOT_TARGET_POS_MAP.get(robotSn);
//        List<String> firstPath = firstRobot.getPathList();
//        for (String key : ROBOT_TARGET_POS_MAP.keySet()) {
//            if (key.equalsIgnoreCase(robotSn)) {
//                continue;
//            }
//            RobotApplyPathDto nextRobotPath = ROBOT_TARGET_POS_MAP.get(key);
//            List<String> keyPath = nextRobotPath.getPathList();
//            List<String> commonPath = firstPath.stream().filter(keyPath::contains).toList();
//            List<String> commonPath2 = keyPath.stream().filter(firstPath::contains).toList();
//            //如果真实路径无公共路径，则找出路径点上转弯点的临近点，组成路径，再算公共路径
//            if (CollectionUtil.isNotEmpty(commonPath)) {
//                // 查询方向
//                String startPos = commonPath.get(0);
//                String endPos = commonPath.get(commonPath.size() - 1);
//                RobotCommonPathDto commonPathDto = new RobotCommonPathDto();
//                commonPathDto.setRobotSnA(robotSn);
//                commonPathDto.setAvoidNum(0);
//                commonPathDto.setRobotSnB(key);
//                commonPathDto.setStartPos(startPos);
//                commonPathDto.setEndPos(endPos);
//                commonPathDto.setSameDirection(false);
//                commonPathDto.setCommonPathName(UUID.randomUUID().toString());
//                if (startPos.equalsIgnoreCase(endPos) ||
//                        (startPos.equalsIgnoreCase(commonPath2.get(0))
//                                && endPos.equalsIgnoreCase(commonPath2.get(commonPath2.size() - 1)))) {
//                    //同向
//                    commonPathDto.setSameDirection(true);
//                }
//                StringBuilder commonPosCodes = new StringBuilder();
//                for (int i = 0; i < commonPath.size(); i++) {
//                    String commonPos = commonPath.get(i);
//                    commonPosCodes.append(commonPos).append("-");
//                }
//                commonPathDto.setCommonPathPos(commonPosCodes.toString());
//                log.info("****************************************************");
//                log.info("公共路径****:{},{}-{}-{}", commonPathDto.getCommonPathPos()
//                        , commonPathDto.getRobotSnA(), commonPathDto.getRobotSnB(), commonPathDto.getSameDirection());
//                log.info("****************************************************");
//                COMMON_PATH_MAP.put(groupId, commonPathDto.getCommonPathName(), commonPathDto);
//                PublicPathStrategyUtil.ROBOT_COMMON_PATH_STATUS.put(robotSn + "_" + commonPathDto.getCommonPathName(), 0);
//                PublicPathStrategyUtil.ROBOT_COMMON_PATH_STATUS.put(key + "_" + commonPathDto.getCommonPathName(), 0);
//            }
//        }
//    }
//
//}
